package controllers;


// PID class to be written by you
public class PID {

  // Private attribute containing a reference to the PIDParameters
  // currently used

	
	private double I;
	private double v;
	private double e;
	private double y, yOld;
	private double D;
	private double ad, bd;

	private PIDParameters contParameters;


  // Additional attributes

  public PID(String name) {
	  contParameters = new PIDParameters(); 
	  this.I = 0.0;
	  this.v = 0.0;
	  this.e = 0.0;
  }


	public synchronized double calculateOutput(double y, double yref) {
		
		this.y = y;
		e = yref - y;
		D = ad*D - bd*(y - yOld);
		v = contParameters.K*(contParameters.Beta*yref - y) + I + D;
		return v;
	}

  // Updates the controller state. Should use tracking-based anti-windup
  // Called from BeamAndBallRegul.
  public synchronized void updateState(double u) {
	  if (contParameters.integratorOn){
		  I = I + (contParameters.K * contParameters.H / contParameters.Ti) * e + (contParameters.H / contParameters.Tr) * (u - v);
	  }
		  yOld = y;
  }

  // Returns the sampling interval expressed as a long. Explicit type casting
  // needed
  public synchronized long getHMillis() {
	  return (long)(contParameters.H * 1000.0);
  }

  // Sets the PIDParameters. Called from PIDGUI. Must clone newParameters.
	public synchronized void setParameters(PIDParameters newParameters) {
		contParameters = (PIDParameters)newParameters.clone();
		if (!contParameters.integratorOn) {
			I = 0.0;
		}
		ad = contParameters.Td / (contParameters.Td + contParameters.N * contParameters.H);
		bd = contParameters.K * ad * contParameters.N;
	}
	
	public synchronized void reset() {
        I = 0.0;
        D = 0.0;
        yOld = 0.0;
}



	public PIDParameters getParameters() {
		
		return contParameters;
	}

}